import numpy
from scipy import signal, ndimage
import matplotlib.pyplot as plt


# 传入的应该是一个转换好的灰度图片


def my_harris_corner_detector(image, gaussian_k=3, gaussian_sigma=1.5, K_val=0.06):
    sobel_filter_x = numpy.array([[3, 0, -3], [10, 0, -10], [3, 0, -3]],
                                 dtype=numpy.float32)
    sobel_filter_y = numpy.array([[3, 10, 3], [0, 0, 0], [-3, -10, -3]],
                                 dtype=numpy.float32)
    # 获得x方向y方向的一阶
    x_side_sobel = my_convolution(image, sobel_filter_x)
    y_side_sobel = my_convolution(image, sobel_filter_y)

    # 使用高斯滤波器得到两个方向上的模糊过的derivativeOfGaussion
    x_side_derivative = my_convolution(x_side_sobel, gaussian_kernel(gaussian_k, gaussian_sigma))
    y_side_derivative = my_convolution(y_side_sobel, gaussian_kernel(gaussian_k, gaussian_sigma))
    # compute Covariance Matrix
    Ixx = x_side_derivative ** 2
    Iyy = y_side_derivative ** 2
    Ixy = x_side_derivative * y_side_derivative

    R_val_matrix = numpy.zeros((x_side_derivative.shape[0], x_side_derivative.shape[1]), dtype=numpy.float)
    k = 0
    harris_res_list = []

    for i in range(2, y_side_derivative.shape[0] - 2):
        for j in range(2, y_side_derivative.shape[1] - 2):
            covariance_matrix = numpy.zeros((2, 2), dtype=numpy.float)
            covariance_matrix[0, 0] = numpy.sum(Ixx[i - 2:i + 3, j - 2:j + 3])
            covariance_matrix[0, 1] = numpy.sum(Ixy[i - 2:i + 3, j - 2:j + 3])
            covariance_matrix[1, 0] = numpy.sum(Ixy[i - 2:i + 3, j - 2:j + 3])
            covariance_matrix[1, 1] = numpy.sum(Iyy[i - 2:i + 3, j - 2:j + 3])
            lambda1, lambda2 = numpy.linalg.eigvals(covariance_matrix)
            R_val_matrix[i, j] = R_val_cal(lambda1, lambda2, K_val)

    max1 = R_val_matrix.max()
    for i in range(2, y_side_derivative.shape[0] - 2):
        for j in range(2, y_side_derivative.shape[1] - 2):
            if R_val_matrix[i, j] > 0.1 * max1:
                harris_res_list.append((i, j))
                k += 1
    plt.gray()
    plt.imshow(image)
    for dots in harris_res_list:
        if R_val_matrix[dots[0] - 1:dots[0] + 2, dots[1] - 1:dots[1] + 2].max() > R_val_matrix[dots[0], dots[1]]:
            harris_res_list.remove(dots)
            k -= 1
        else:
            plt.scatter([dots[1]], [dots[0]], color='b')
    print(k)

    plt.title('Harris detection image')  # 标题
    plt.show()
    return harris_res_list


def my_convolution(image, a_filter):
    # image_new = signal.fftconvolve(image, a_filter)
    image_new = ndimage.convolve(image, a_filter, None, 'constant', 0.0, 0)
    # image_new = numpy.zeros((image.shape[0], image.shape[1]), dtype=numpy.float)
    # y_limit, x_limit = image.shape
    # y_length, x_length = a_filter.shape
    # m = int((x_length - 1) / 2)
    # n = int((y_length - 1) / 2)
    #
    # for i in range(0, n):
    #     for j in range(0, x_limit):
    #         image_new[i, j] = image[i, j]
    #
    # for i in range(y_limit - n, y_limit):
    #     for j in range(0, x_limit):
    #         image_new[i, j] = image[i, j]
    #
    # for i in range(0, y_limit):
    #     for j in range(0, m):
    #         image_new[i, j] = image[i, j]
    #
    # for i in range(0, y_limit):
    #     for j in range(x_limit - m, x_limit):
    #         image_new[i, j] = image[i, j]
    #
    # for x in range(m, x_limit - m):
    #     for y in range(n, y_limit - n):
    #         sum1 = 0
    #         for small_x in range(x - m, x + m + 1):
    #             for small_y in range(y - n, y + n + 1):
    #                 sum1 += image[small_y, small_x] * a_filter[small_y - y + n, small_x - x + m]
    #                 image_new[y, x] = sum1
    return image_new


def gaussian_kernel(K_size=3, sigma=1.2):
    pad = K_size // 2
    K = numpy.zeros((K_size, K_size), dtype=numpy.float)
    for x in range(-pad, -pad + K_size):
        for y in range(-pad, -pad + K_size):
            K[y + pad, x + pad] = numpy.exp(-(x ** 2 + y ** 2) / (2 * (sigma ** 2)))
    K /= (2 * numpy.pi * sigma * sigma)
    K /= K.sum()
    return K


def R_val_cal(lambda1, lambda2, K):
    return lambda1 * lambda2 - K * ((lambda1 + lambda2) ** 2)
